
#include "digit.hpp"

#include <opencv2/opencv.hpp>
#include <opencv2/text.hpp>

#include "server.hpp"

static cv::Mat ToShowMat(const cv::Mat &m)
{
    double val[2];
    cv::minMaxIdx(m, val, val+1);
    cv::Mat m8;
    cv::convertScaleAbs(m, m8, 255/(val[1]-val[0]), -val[0]*255/(val[1]-val[0]));
    return m8;
}


void ThinSubiteration1(cv::Mat & pSrc, cv::Mat & pDst) {
    int rows = pSrc.rows;
    int cols = pSrc.cols;
    pSrc.copyTo(pDst);
    for(int i = 0; i < rows; i++) {
        for(int j = 0; j < cols; j++) {
            if(pSrc.at<float>(i, j) == 1.0f) {
                /// get 8 neighbors
                /// calculate C(p)
                int neighbor0 = (int) pSrc.at<float>( i-1, j-1);
                int neighbor1 = (int) pSrc.at<float>( i-1, j);
                int neighbor2 = (int) pSrc.at<float>( i-1, j+1);
                int neighbor3 = (int) pSrc.at<float>( i, j+1);
                int neighbor4 = (int) pSrc.at<float>( i+1, j+1);
                int neighbor5 = (int) pSrc.at<float>( i+1, j);
                int neighbor6 = (int) pSrc.at<float>( i+1, j-1);
                int neighbor7 = (int) pSrc.at<float>( i, j-1);
                int C = int(~neighbor1 & ( neighbor2 | neighbor3)) +
                        int(~neighbor3 & ( neighbor4 | neighbor5)) +
                        int(~neighbor5 & ( neighbor6 | neighbor7)) +
                        int(~neighbor7 & ( neighbor0 | neighbor1));
                if(C == 1) {
                    /// calculate N
                    int N1 = int(neighbor0 | neighbor1) +
                            int(neighbor2 | neighbor3) +
                            int(neighbor4 | neighbor5) +
                            int(neighbor6 | neighbor7);
                    int N2 = int(neighbor1 | neighbor2) +
                            int(neighbor3 | neighbor4) +
                            int(neighbor5 | neighbor6) +
                            int(neighbor7 | neighbor0);
                    int N = std::min(N1,N2);
                    if ((N == 2) || (N == 3)) {
                        /// calculate criteria 3
                        int c3 = ( neighbor1 | neighbor2 | ~neighbor4) & neighbor3;
                        if(c3 == 0) {
                            pDst.at<float>( i, j) = 0.0f;
                        }
                    }
                }
            }
        }
    }
}


void ThinSubiteration2(cv::Mat & pSrc, cv::Mat & pDst) {
    int rows = pSrc.rows;
    int cols = pSrc.cols;
    pSrc.copyTo( pDst);
    for(int i = 0; i < rows; i++) {
        for(int j = 0; j < cols; j++) {
            if (pSrc.at<float>( i, j) == 1.0f) {
                /// get 8 neighbors
                /// calculate C(p)
                int neighbor0 = (int) pSrc.at<float>( i-1, j-1);
                int neighbor1 = (int) pSrc.at<float>( i-1, j);
                int neighbor2 = (int) pSrc.at<float>( i-1, j+1);
                int neighbor3 = (int) pSrc.at<float>( i, j+1);
                int neighbor4 = (int) pSrc.at<float>( i+1, j+1);
                int neighbor5 = (int) pSrc.at<float>( i+1, j);
                int neighbor6 = (int) pSrc.at<float>( i+1, j-1);
                int neighbor7 = (int) pSrc.at<float>( i, j-1);
                int C = int(~neighbor1 & ( neighbor2 | neighbor3)) +
                        int(~neighbor3 & ( neighbor4 | neighbor5)) +
                        int(~neighbor5 & ( neighbor6 | neighbor7)) +
                        int(~neighbor7 & ( neighbor0 | neighbor1));
                if(C == 1) {
                    /// calculate N
                    int N1 = int(neighbor0 | neighbor1) +
                            int(neighbor2 | neighbor3) +
                            int(neighbor4 | neighbor5) +
                            int(neighbor6 | neighbor7);
                    int N2 = int(neighbor1 | neighbor2) +
                            int(neighbor3 | neighbor4) +
                            int(neighbor5 | neighbor6) +
                            int(neighbor7 | neighbor0);
                    int N = std::min(N1,N2);
                    if((N == 2) || (N == 3)) {
                        int E = (neighbor5 | neighbor6 | ~neighbor0) & neighbor7;
                        if(E == 0) {
                            pDst.at<float>(i, j) = 0.0f;
                        }
                    }
                }
            }
        }
    }
}


void normalizeLetter(cv::Mat & inputarray, cv::Mat & outputarray) {
    bool bDone = false;
    int rows = inputarray.rows;
    int cols = inputarray.cols;
    
    inputarray.convertTo(inputarray,CV_32FC1);
    
    inputarray.copyTo(outputarray);
    
    outputarray.convertTo(outputarray,CV_32FC1);
    
    /// pad source
    cv::Mat p_enlarged_src = cv::Mat(rows + 2, cols + 2, CV_32FC1);
    for(int i = 0; i < (rows+2); i++) {
        p_enlarged_src.at<float>(i, 0) = 0.0f;
        p_enlarged_src.at<float>( i, cols+1) = 0.0f;
    }
    for(int j = 0; j < (cols+2); j++) {
        p_enlarged_src.at<float>(0, j) = 0.0f;
        p_enlarged_src.at<float>(rows+1, j) = 0.0f;
    }
    for(int i = 0; i < rows; i++) {
        for(int j = 0; j < cols; j++) {
            if (inputarray.at<float>(i, j) >= 20.0f) {
                p_enlarged_src.at<float>( i+1, j+1) = 1.0f;
            }
            else
                p_enlarged_src.at<float>( i+1, j+1) = 0.0f;
        }
    }
    
    /// start to thin
    cv::Mat p_thinMat1 = cv::Mat::zeros(rows + 2, cols + 2, CV_32FC1);
    cv::Mat p_thinMat2 = cv::Mat::zeros(rows + 2, cols + 2, CV_32FC1);
    cv::Mat p_cmp = cv::Mat::zeros(rows + 2, cols + 2, CV_8UC1);
    
    while (bDone != true) {
        /// sub-iteration 1
        ThinSubiteration1(p_enlarged_src, p_thinMat1);
        /// sub-iteration 2
        ThinSubiteration2(p_thinMat1, p_thinMat2);
        /// compare
        cv::compare(p_enlarged_src, p_thinMat2, p_cmp, CV_CMP_EQ);
        /// check
        int num_non_zero = cv::countNonZero(p_cmp);
        if(num_non_zero == (rows + 2) * (cols + 2)) {
            bDone = true;
        }
        /// copy
        p_thinMat2.copyTo(p_enlarged_src);
    }
    // copy result
    for(int i = 0; i < rows; i++) {
        for(int j = 0; j < cols; j++) {
            outputarray.at<float>( i, j) = p_enlarged_src.at<float>( i+1, j+1);
        }
    }
}

static cv::Mat Skeleton(const cv::Mat &bw)
{
    
    cv::Mat img=bw.clone();
    
    cv::Mat skel(img.size(), CV_8UC1, cv::Scalar(0));
    cv::Mat temp;
    cv::Mat eroded;
    
    cv::Mat element = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(3, 3));
    
    bool done;
    do
    {
        cv::erode(img, eroded, element);
        cv::dilate(eroded, temp, element); // temp = open(img)
        cv::subtract(img, temp, temp);
        cv::bitwise_or(skel, temp, skel);
        eroded.copyTo(img);
        
        done = (cv::countNonZero(img) == 0);
    } while (!done);
    
    return skel;
}

static cv::Mat Skeleton2(const cv::Mat &bw)
{
    cv::Mat in=bw.clone();
    cv::Mat out;
    normalizeLetter(in,out);
    std::cout<<out<<'\n';
    cv::Mat out8;
    out.convertTo(out8, CV_8UC1, 255);
    return out8;
}

static cv::Point LP(const cv::Mat &bw)
{
    cv::Point pt(0,bw.rows-1);
    
    for(;pt.y>=0;pt.y--)
    {
        for(pt.x=0;pt.x<bw.cols;pt.x++)
        {
            if(bw.at<uchar>(pt)>0)
                return pt;
        }
    }
    return pt;
}



static cv::Point LP2(const cv::Mat &bw)
{
    cv::Mat edge;
    cv::Canny(bw, edge, 1, 3);
    float hratio=0.15;
    cv::Rect roi(0, (0.75-hratio)*edge.rows, edge.cols/2, edge.rows*2*hratio);
    cv::namedWindow("roi", cv::WINDOW_NORMAL);
    cv::imshow("roi", edge(roi));
    
    return cv::Point();
    
}


static cv::Vec3d pt2ray(cv::Vec2d pt, double angle, const cv::Mat &K, const cv::Mat &D)
{
    cv::Mat R;
    cv::Mat rv(3, 1, CV_64FC1, cv::Scalar(0));
    rv.at<double>(0)=angle;
    cv::Rodrigues(rv, R);

    std::vector<cv::Vec2d> inl(1, pt);
    std::vector<cv::Vec2d> outl;
    cv::undistortPoints(inl,outl, K, D, R);

    return cv::Vec3d(outl[0][0], outl[0][1], 1);
}


static cv::Vec3d ray2center(cv::Vec3d ray, double yheight)
{
    return (yheight/ray[1])*ray;
}


static cv::Vec3d pts2loc(cv::Vec2d pt1, cv::Vec2d pt2, double angle, double yheight, const cv::Mat &K, const cv::Mat &D)
{
    cv::Vec3d ct1=ray2center(pt2ray(pt1, angle, K, D), yheight);
    cv::Vec3d ct2=ray2center(pt2ray(pt2, angle, K, D), yheight);

    cv::Vec3d ct0=(ct1+ct2)/2;

    double rad=atan2(ct1[2]-ct2[2], ct1[0]-ct2[0]);

    return cv::Vec3d(ct0[0],ct0[2],rad);
}


#include "usbcamera.hpp"

#include <thread>
#include <chrono>

void test22()
{

//    to check stm echo message, install gtkterm
//            sudo apt-get install gtkterm
//            set port and baudrate on menu item 'Configuration'-'Port'
//            port="/dev/ttyACM0"
//            baudrate=115200


    FILE *file = NULL;

    file=fopen("/dev/ttyACM0","w");


    for(int i=0;i<1;i++)
    {
            if(file!=NULL)
                        {
                            fprintf(file, "@mfr,35,315!\n");
                            fflush(file);

                        }
                        else
                        {
                            printf("null file\n");
                        }
//                std::cout<<i<<"\tnot found\n";
//            std::cout<<"\tnot found\n";

           std::this_thread::sleep_for(std::chrono::milliseconds(1000));



    }



    if(file!=NULL)
                {
                    fprintf(file, "@mtr,18,0!\n");
                    fflush(file);

                }
                else
                {
                    printf("null file\n");
                }


    if(file!=NULL)
                {
                    fprintf(file, "@mss,0,0!\n");
                    fflush(file);

                }
                else
                {
                    printf("null file\n");
                }

    printf("file close\n");
    fclose(file);

}




int main(int argc, char* argv[])

{

//    ServerBase().start();

    test22();

//    USBCamera::Calib("20171010_112736/");

//    USBCamera().test(1);
    
//    Digit::test();
//    Digit::pic();
    
    return 0;
    
    cv::Mat m = cv::imread("3.png",0);
    
    cv::Mat res;
    
    cv::cornerHarris(m, res, 5, 7, 0.04,
                     //                 cv::BORDER_DEFAULT
                     cv::BORDER_CONSTANT
                     );
    
    cv::threshold(m, m, 255, 255, cv::THRESH_OTSU|cv::THRESH_BINARY);
    //cv::compare(m,120,m,cv::CMP_GT);
    
    cv::Point lp=LP(m);
    
    std::cout<<lp<<"\n";
    
    cv::Mat sk=Skeleton2(m);
    
    cv::namedWindow("sk", cv::WINDOW_NORMAL);
    cv::imshow("sk", sk);
    //cv::waitKey();
    
    
    //cv::blur(m,m,cv::Size(2,2));
    
    cv::Mat me;
    cv::Canny(m,me,1,3);
    
    cv::namedWindow("me", cv::WINDOW_NORMAL);
    cv::imshow("me", me);
    
    
    cv::namedWindow("m", cv::WINDOW_NORMAL);
    cv::imshow("m", m);
    
    
    
    cv::Mat res8=ToShowMat(res);
    
    cv::Mat resd;
    
    int r=m.rows/5;
    
    cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT,cv::Size(r,r));
    cv::dilate(res,resd,element);
    
    cv::Mat res2;
    
    cv::compare(res,resd,res2,cv::CMP_EQ);
    
    
    std::cout<<res2<<"\n\n\n"<<std::flush;
    
    
    
    cv::Mat ml[3]={m,res2,sk};
    cv::Mat res88;
    cv::merge(ml,3,res88);
    
    cv::namedWindow("res8", cv::WINDOW_NORMAL);
    
    cv::imshow("res8",res88);
    
    std::cout<<res8<<"\n"<<std::flush;
    
    
    cv::waitKey(300000);
    return 0;
    
}
